/********************************************************************
	created:	1/11/2005   20:40
	filename: 	glmath.cpp
	author:		GMKal (exile79@gmail.com)
	
	purpose:	vector, matrix, quatrnion classes and methods

    last modified:   6/11/2005   20:08
*********************************************************************/


#include "stdafx.h"

#include "glmath.h"


using namespace glmath;


/************************************************************************/
/* Vector3 class														*/
/*																		*/
/*																		*/
/*																		*/
/************************************************************************/


const Vector3 Vector3::ZERO( 0, 0, 0 );

const Vector3 Vector3::UNIT_X( 1, 0, 0 );
const Vector3 Vector3::UNIT_Y( 0, 1, 0 );
const Vector3 Vector3::UNIT_Z( 0, 0, 1 );
const Vector3 Vector3::NEGATIVE_UNIT_X( -1,  0,  0 );
const Vector3 Vector3::NEGATIVE_UNIT_Y(  0, -1,  0 );
const Vector3 Vector3::NEGATIVE_UNIT_Z(  0,  0, -1 );
const Vector3 Vector3::UNIT_SCALE(1, 1, 1);


void Vector3::Set(float x,float y,float z)
{
	this->x=x;	
	this->y=y;	
	this->z=z;
}


void Vector3::Set(float* array)
{
	memcpy(&x,array,sizeof(float)*3);
}

/*
 *	return the cross product of (*this) with vector v
 */
Vector3 Vector3::CrossProduct(const Vector3& v) const
{
	return Vector3(
			y*v.z - z*v.y,
			z*v.x - x*v.z,
			x*v.y - y*v.x
			);	
}


/*
 *	dot-product
 *	The dot product is this equation: 
 *	V1.V2 = (V1.x * V2.x  +  V1.y * V2.y  +  V1.z * V2.z)
 *	In math terms, it looks like this:  V1.V2 = ||V1|| ||V2|| cos(theta)
 *	The '.' means DOT.   The || || is magnitude.  So the magnitude of V1 times 
 *	the magnitude of V2 times the cosine of the angle.  
 *	This function is used for a ton of things. For instance, we use it to compute 
 *	the angle between 2 vectors.  If the vectors are normalized, the dot product 
 *	returns the cosine of the angle between the 2 vectors. To get the actual angle 
 *	we use the arc cosine. 
 *	There is more on this in the below function AngleBetweenVectors().  
 *	Let's give some applications of using the dot product.  How would you tell if 
 *	the angle between the 2 vectors is perpendicular (90 degrees)?  Well, if we 
 *	normalize the vectors we can get rid of the ||V1|| * ||V2|| in front, which 
 *	just leaves us with:  cos(theta). If a vector is normalized, it's magnitude 
 *	is 1, so it would be: 1 * 1 * cos(theta), which is pointless, so we discard 
 *	that part of the equation. So, What is the cosine of 90? If you punch it in 
 *	your calculator you will find that it's 0.  So that means if the dot product 
 *	of 2 angles is 0, then they are perpendicular.  What we did in our mind is 
 *	take the arc cosine of 0, which is 90 (or PI/2 in radians).  
 */
float Vector3::DotProduct(const Vector3& v) const
{
	return x*v.x + y*v.y + z*v.z;
}

/*
 *	get the length of the vector
 */
float Vector3::Length() const
{	
	return (float)sqrt(x*x+y*y+z*z);
}

/*
 *	get the length squared
 */
float Vector3::LengthSquared() const
{
	return x*x+y*y+z*z;
}

/*
 *	normalize the vector
 */
void Vector3::Normalize()
{
	float ilen = 1.0f/Length();
	x *= ilen;
	y *= ilen;
	z *= ilen;
}

/*
 *	return the vector normalized 
 */
Vector3 Vector3::Normalized() const
{
	Vector3 result(*this);
	result.Normalize();
	return result;
}

/*
 *	interpolate the vector between v1 and v2 with interpolation 
 *	value = r
 */
void Vector3::Lerp(const Vector3& v1,const Vector3& v2,const float r)
{
	float ir = 1.0f-r;
	x = v1.x*ir + v2.x*r;
	y = v1.y*ir + v2.y*r;
	z = v1.z*ir + v2.z*r;	
}

/*
 *	return true if all equal
 */
bool Vector3::operator ==(const Vector3& v) const
{
	return (x==v.x && y==v.y && z==v.z);
}

/*
 *	return true if at least one different
 */
bool Vector3::operator !=(const Vector3& v) const
{
	return (x!=v.x || y!=v.y || z!=v.z);
}

/*
 *	negate
 */
Vector3 Vector3::operator -() const
{	
	return Vector3(-x,-y,-z); 
}

/*
 *	increment
 */
void Vector3::operator +=(const Vector3& v)
{	
	x += v.x;	
	y += v.y;	
	z += v.z;	
}

/*
 *	decrement
 */
void Vector3::operator -=(const Vector3& v)
{	
	x -= v.x;	
	y -= v.y;	
	z -= v.z;	
}

/*
 *	self-multiply
 */
void Vector3::operator *=(const float f)
{
	x *= f;
	y *= f;
	z *= f;
}

void Vector3::operator /=(const float f)
{
	x /= f;
	y /= f;
	z /= f;
}

/*
 *	addition
 *	generally, prefer the add(const Vector3& v1,const Vector3& v2)
 *	instead of this to avoid the temporary constructor
 */
Vector3 Vector3::operator+(const Vector3& v) const
{
	return Vector3(
		x+v.x, y+v.y, z+v.z	
		);
}

/*
 *	subtraction
 */
Vector3 Vector3::operator-(const Vector3& v) const
{
	return Vector3(
		x-v.x, y-v.y, z-v.z	
		);
}

/*
 *	post-multiplication with scalar (vector*scalar)
 */
Vector3 Vector3::operator *(const float f) const
{
	return Vector3(
		x*f, y*f, z*f
		);
}

/*
 *	post-division with scalar
 */
Vector3 Vector3::operator /(float f) const
{
	return Vector3(
		x/f, y/f, z/f
		);
}

/*
 *	pre-multiply by a scalar (scalar*vector)
 */
Vector3 glmath::operator*(const float r,const Vector3& v)
{
	return v*r;
}


/*
 *	vector addition
 */
Vector3 glmath::VectorAdd(const Vector3& v1,const Vector3& v2)
{
	return Vector3(
		v1.x+v2.x,
		v1.y+v2.y,
		v1.z+v2.z
		);
}

/*
 *	use this instead of this = v1+v2 for speed, avoids the 
 *	temporary constructor
 */
void glmath::VectorAdd(const Vector3& v1,const Vector3& v2, Vector3& vsum)
{
	vsum.Set(
		v1.x+v2.x,
		v1.y+v2.y,
		v1.z+v2.z
		);
}


/*
 *	vector subtraction
 */
Vector3 glmath::VectorSubtract(const Vector3& v1,const Vector3& v2)
{
	return Vector3(
		v1.x-v2.x,
		v1.y-v2.y,
		v1.z-v2.z
		);
}


void glmath::VectorSubtract(const Vector3& v1,const Vector3& v2,Vector3& vdiff)
{
	vdiff.Set(
		v1.x-v2.x,
		v1.y-v2.y,
		v1.z-v2.z
		);
}

/*
 *	get dot product of two vectors
 *	the dot product returns the cosine of the angle between 2 
 *	vectors. Well, that is assuming they are unit vectors (normalized vectors).
 */
float glmath::Dot3(const Vector3& v1,const Vector3& v2)
{
	return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
}

/*
 *	This returns a perpendicular vector from 2 given vectors by taking the 
 *	cross product.
 */
Vector3 glmath::Cross3(const Vector3& v1,const Vector3& v2)
{
	return Vector3( 
		v1.y*v2.z - v1.z*v2.y,
		v1.z*v2.x- v1.x*v2.z,
		v1.x*v2.y - v1.y*v2.x 
		);
}

void glmath::Cross3(const Vector3& v1,const Vector3& v2,Vector3& vcross)
{
	vcross.Set( 
		v1.y*v2.z - v1.z*v2.y,
		v1.z*v2.x- v1.x*v2.z,
		v1.x*v2.y - v1.y*v2.x	
		);
}

/*
 *	distance between two vectors
 */
float glmath::Distance(const Vector3& v1,const Vector3& v2)
{
	return (v1-v2).Length();
}



/*
 *	the dot product returns the cosine of the angle between 2 vectors.
 *	Well, that is assuming they are unit vectors (normalized vectors).
 *	So, if we don't have a unit vector, then instead of just saying  
 *	arcCos(DotProduct(A, B))
 *	we need to divide the dot product by the magnitude of the 2 vectors multiplied 
 *	by each other. Here is the equation:  arc cosine of (V . W / || V || * || W || )
 *	the || V || means the magnitude of V.  This then cancels out the magnitudes dot 
 *	product magnitudes. But basically, if you have normalize vectors already, you 
 *	can forget about the magnitude part.	
 */
float glmath::RadiansBetweenVectors(const Vector3& v1,const Vector3& v2)
{
	float dotp = glmath::Dot3(v1,v2);

	float angle = acos( dotp / (v1.Length()*v2.Length()));

	// Here we make sure that the angle is not a -1.#IND0000000 number, which means 
	// indefinite. acos() thinks it's funny when it returns -1.#IND0000000.  If we 
	// don't do this check, our collision results will sometimes say we are colliding 
	// when we aren't.  I found this out the hard way after MANY hours and already 
	// wrong written tutorials :)  Usually this value is found when the dot product 
	// and the magnitude are the same value. We want to return 0 when this happens.
	if (_isnan(angle))
	{	return 0;	}

	return angle;	
}


/*
 *	OpenGL helpers
 */

void glmath::glVertex3(const Vector3& v)
{
	::glVertex3f(v.x,v.y,v.z);
}


void glmath::glNormal3(const Vector3& v)
{
	::glNormal3f(v.x,v.y,v.z);
}

void glmath::glTranslate(const Vector3& v)
{
	::glTranslatef(v.x,v.y,v.z);
}




/************************************************************************/
/* vector2 class
*/
/************************************************************************/

void Vector2::Set(float x,float y,float z/* =0 */)
{
	this->x=x; 
	this->y=y; 
}

void Vector2::Set(const float* array)
{
	this->x=array[0]; 
	this->y=array[1]; 
}


/*
 *	return the cross product of this with vector v
 *	they are 2d vectors so the result is always (0,0,1)	
 */
Vector3 Vector2::CrossProduct(const Vector2& v) const
{
	return Vector3(0,0,1);
}

/*
 *	dot product
 */
float Vector2::DotProduct(const Vector2& v) const
{
	return x*v.x + y*v.y;
}


/*
 *	
 */
float Vector2::Length() const
{
	return (float)sqrt(x*x+y*y);
}

float Vector2::LengthSquared() const
{
	return x*x + y*y;
}

/*
 *	return a normalized copy of this vector
 */
Vector2 Vector2::Normalized() const 
{
	return (*this)/this->Length();
}

/*
 *	normalize this vector
 */
void Vector2::Normalize()
{
	(*this) /= this->Length();	
}

/*
 *	interpolate the vector between v1 and v2 with 
 *	interpolation value = r
 */
const Vector2& Vector2::Lerp(const Vector2& v1,const Vector2& v2,const float r)
{
	float ir = 1.0f-r;
	x = v1.x*ir + v2.x*r;
	y = v1.y*ir + v2.y*r;	
	return *this;
}


/*
 *	return true if all elements are equal
 */
bool Vector2::operator== (const Vector2& v) const
{
	return (x==v.x && y==v.y);
}

/*
 *	return true if at least one element differs
 */
bool Vector2::operator != (const Vector2& v) const
{
	return (x!=v.x || y!=v.y);
}

/*
 *	negate
 */
Vector2 Vector2::operator-() const 
{ 
	return Vector2(-x,-y); 
}

/*
 *	increment
 */
void Vector2::operator+= (const Vector2& v)
{
	x += v.x;
	y += v.y;
}

/*
 *	decrement
 */
void Vector2::operator-= (const Vector2& v)
{
	x -= v.x;
	y -= v.y;
}


void Vector2::operator*= (const float r)
{	
	x *= r;	
	y *= r;	
}

void Vector2::operator /=(const float r)
{	
	x /= r;	
	y /= r;	
}

/*
 *	addition
 *	generally, prefer the add(const Vector3& v1,const Vector3& v2)
 *	instead of this to avoid the temporary constructor
 */
Vector2 Vector2::operator+(const Vector2& v) const
{
	return Vector2(
		x+v.x,
		y+v.y
		);
}

Vector2 Vector2::operator-(const Vector2& v) const
{
	return Vector2(
		x-v.x,
		y-v.y
		);
}

/*
 *	post-multiplication with scalar
 */
Vector2 Vector2::operator*(const float r) const
{
	return Vector2(
		x*r,
		y*r
		);	
}

/*
 *	post-division with scalar
 */
Vector2 Vector2::operator/(const float r) const
{
	return Vector2(
		x/r,
		y/r
		);	
}


/*
 *	pre-multiply by a scalar
 */
Vector2 glmath::operator*(const float r,const Vector2& v)
{
	return v*r;
}


/*
 *	vector addition
 */
Vector2 glmath::VectorAdd(const Vector2& v1,const Vector2& v2)
{
	return Vector2(
		v1.x + v2.x,
		v1.y + v2.y
		);	
}

void glmath::VectorAdd(const Vector2& v1,const Vector2& v2,Vector2& vsum)
{
	vsum.Set(
		v1.x + v2.x,
		v1.y + v2.y
		);
}



Vector2 glmath::VectorSubtract(const Vector2& v1,const Vector2& v2)
{
	return Vector2(
		v1.x - v2.x,
		v1.y - v2.y
		);	
}

void glmath::VectorSubtract(const Vector2& v1,const Vector2& v2,Vector2& vdiff)
{
	vdiff.Set(
		v1.x - v2.x,
		v1.y - v2.y
		);
}


/*
 *	opengl helpers
 */
void glmath::glVertex2(const Vector2& v)
{
	::glVertex2f(v.x,v.y);
}

void glmath::glTexCoord2(const Vector2& v)
{
	::glTexCoord2f(v.x,v.y);
}


/************************************************************************/
/* quaternion class

*/
/************************************************************************/

Quaternion::Quaternion(float x/* =0.0 */,float y/* =0.0 */, 
					   float z/* =0.0 */,float w/* =1.0 */)
: x(x),y(y),z(z),w(w) 
{
}

Quaternion::Quaternion(const Quaternion& q1,const Quaternion& q2,float& interp)
{
	Slerp(q1,q2,interp);
}



void Quaternion::Set(float x1,float y1, float z1,float w1)
{	
	x=x1;	y=y1;	z=z1;	w=w1;	
}

/*
 *	reset quaternion to identity
 */
void Quaternion::Identity()
{	
	Set(0,0,0,1);	
}


/*
 *	get the magnitude of the quaternion
 */
float Quaternion::Length() const
{
	return float(sqrt(x*x+y*y+z*z+w*w));
}

/*
 *	get the magnitude of the quaternion *squared*
 */
float Quaternion::SqrLength() const
{
	return float(x*x+y*y+z*z+w*w);
}

/*
 *	set quaternion to describe a rotation around a specified axis
 */
const Quaternion& Quaternion::SetFromAxis(double radians,float xaxis,float yaxis,float zaxis)
{
	// normalize the axis
	float factor = (float)sqrt(xaxis*xaxis+yaxis*yaxis+zaxis*zaxis);
	xaxis /= factor;
	yaxis /= factor;
	zaxis /= factor;

	float sina = float(sin(radians*0.5f));
	float cosa = float(cos(radians*0.5f));

	Set(xaxis*sina, yaxis*sina, zaxis*sina, cosa);
	return *this;
}

const Quaternion& Quaternion::SetFromAxis(double radians,const Vector3& axis)
{
	return SetFromAxis(radians,axis.x,axis.y,axis.z);
}


/*
 *	normalize the quaternion
 */
void Quaternion::Normalize()
{
	float len = Length();
	if(len==0)
	{
		Identity();		
	}
	else
	{
		float ilen = 1.0f/len;
		(*this) *= ilen;
	}
}

/*
 *	return the conjugate quat
 */
Quaternion Quaternion::Conjugate()
{
	return Quaternion(-x,-y,-z,w);
}

/*
 *	return the cross product of 2 quaternions (this and q)
 */
Quaternion Quaternion::CrossProduct(const Quaternion& q) const
{
	return Quaternion(
		w * q.x + x * q.w + y * q.z - z * q.y,
		w * q.y + x * q.z + y * q.w - z * q.x,
		w * q.z + x * q.y + y * q.x - z * q.w,
		w * q.w - x * q.x - y * q.y - z * q.z
	);
}

/*
 *	set the quaternion from 3 angles 
 *	its the same if we had created 3 quaternions, each created
 *	with SetFromAxis around the 3 different axis and then multiply them
 */
const Quaternion& Quaternion::SetFromRadians(double xradians,double yradians,double zradians)
{
	float cx((float)cos(xradians*0.5f));
	float sx((float)sin(xradians*0.5f));
	float cy((float)cos(yradians*0.5f));
	float sy((float)sin(yradians*0.5f));
	float cz((float)cos(zradians*0.5f));
	float sz((float)sin(zradians*0.5f));
	Set(	sx*cy*cz - cx*sy*sz,
			cx*sy*cz + sx*cy*sz,
			cx*cy*sz  - sx*sy*cz,
			cx*cy*cz  + sx*sy*sz );
	return *this;
}



/*
 *	create a rotation matrix from the quaternion
 */
void Quaternion::CreateMatrix(float mat[16],bool bNormalize/* =true */)
{
	// Error checking...
	if(!mat)
		return;

	if(bNormalize)
		Normalize();

	// Calculate the first row.
	mat[0]  = 1.0f - 2.0f * (y * y + z * z); 
	mat[1]  = 2.0f * (x * y + z * w);
	mat[2]  = 2.0f * (x * z - y * w);
	mat[3]  = 0.0f;  

	mat[4]  = 2.0f * (x * y - z * w);  
	mat[5]  = 1.0f - 2.0f * (x * x + z * z); 
	mat[6]  = 2.0f * (z * y + x * w);  
	mat[7]  = 0.0f;  

	mat[8]  = 2.0f * (x * z + y * w);
	mat[9]  = 2.0f * (y * z - x * w);
	mat[10] = 1.0f - 2.0f * (x * x + y * y);  
	mat[11] = 0.0f;  

	mat[12] = 0;  
	mat[13] = 0;  
	mat[14] = 0;  
	mat[15] = 1.0f;
}

/*
 *	
 */
Matrix Quaternion::CreateMatrix(bool bNormalize/* =true */)
{
	Matrix mat;
	CreateMatrix(mat.data, bNormalize);
	return mat;
}

/*
 *	inverse quaternion
 *	.....just negate all its elements
 *	same as (*this) * -1
 */
void Quaternion::Inverse()
{	
	x=-x; 
	y=-y; 
	z=-z; 
	w=-w; 
}

/*
 *	slerp -- spherical linear interpolation
 */
void Quaternion::Slerp(const Quaternion& q1,const Quaternion& q2,float interp)
{
	float cosTheta = 0.0f;
	float sinTheta = 0.0f;
	float beta = 0.0f;
	Quaternion tmpq2(q2);	

	cosTheta = q1.x*tmpq2.x + q1.y*tmpq2.y + q1.z*tmpq2.z + q1.w*tmpq2.w;

	if(cosTheta < 0.0f)
	{
		// Flip sigh if so.
		tmpq2.Inverse();
		cosTheta = -cosTheta;
	}

	//beta = 1.0f - interp;
	
	float sclq1, sclq2;
	if(1.0f+cosTheta >0.00000001f)
	{
		if(1.0f - cosTheta > 0.00000001f)
		{
			// We are using spherical interpolation.
			float theta = (float)acos(cosTheta);
			sinTheta = (float)sin(theta);
			sclq1 = (float)sin(theta * (1.0f-interp))/sinTheta;
			sclq2 = (float)sin(theta * interp) / sinTheta;
		}
		else
		{
			sclq1 = 1.0f-interp;
			sclq2 = interp;
		}

		// Interpolation.
		x = sclq1*q1.x + sclq2*tmpq2.x;
		y = sclq1*q1.y + sclq2*tmpq2.y;
		z = sclq1*q1.z + sclq2*tmpq2.z;
		w = sclq1*q1.w + sclq2*tmpq2.w;
	}
	else
	{
		x = -q1.y;
		y =  q1.x;
		z = -q1.w;
		w =  q1.z;

		sclq1 = (float)sin(( 1.0f-interp )*PIdiv2 );
		sclq2 = (float)sin( interp*PIdiv2 );
		
		x =  sclq1*q1.x + sclq2*x;
		y =  sclq1*q1.y + sclq2*y;
		z =  sclq1*q1.z + sclq2*z;
		w =  sclq1*q1.w + sclq2*w;
	}

}


/*
 *	quaternion multiplication
 */
Quaternion Quaternion::operator*(const Quaternion& q) const
{
	return Quaternion(
		w * q.x + x * q.w + y * q.z - z * q.y,
		w * q.y - x * q.z + y * q.w + z * q.x,
		w * q.z + x * q.y - y * q.x + z * q.w,
		w * q.w - x * q.x - y * q.y - z * q.z
		);
}

void Quaternion::operator *=(const Quaternion& q)
{
	(*this) = (*this)*q;
}

void Quaternion::operator *=(const float f)
{
	x *= f;
	y *= f;
	z *= f;
	w *= f;
}

Quaternion Quaternion::operator*(const float f) const
{
	return Quaternion(x*f,y*f,z*f,w*f);	
}


Quaternion Quaternion::operator+(const Quaternion& q) const
{
	Vector3 v1(x,y,z);
	Vector3 v2(q.x,q.y,q.z);

	Vector3 vcross = Cross3(v1,v2);
	v1 *= q.w;
	v2 *= w;

	Vector3 v3 = v1+v2+vcross;
	
	float tmpw = w * q.w - (x * q.x + y * q.y + z * q.z);

    return Quaternion(v3.x,v3.y,v3.z,tmpw);
}

/*
 *	rotate a vector...
 */
void Quaternion::RotateVector(Vector3& v, bool bNormalize/* =true */)
{
	GLfloat rotmat[16];
	CreateMatrix(rotmat,bNormalize);

	Vector3 vtmp(
		rotmat[0]*v.x + rotmat[4]*v.y + rotmat[ 8]*v.z,
		rotmat[1]*v.x + rotmat[5]*v.y + rotmat[ 9]*v.z,
		rotmat[2]*v.x + rotmat[6]*v.y + rotmat[10]*v.z );
	v = vtmp;
}


/*
 *	
 */
Vector3 Quaternion::GetDirectionVector() const
{
	/*return Vector3(
		2.0f * (x*z - w*y),
		2.0f * (y*z + w*x),
		1.0f - 2.0f*(x*x + y*y)
		);*/

	return Vector3(
		2.0f * (x*z + w*y),
		2.0f * (y*z - w*x),
		1.0f - 2.0f*(x*x + y*y)
		);
}

/************************************************************************/
/* matrix class


*/
/************************************************************************/


/*
 *	create an identity matrix
 */
Matrix::Matrix()
{
	Identity();
}


/*
 *	create a matrix with specified each element
 */
Matrix::Matrix(GLfloat _00, GLfloat _01, GLfloat _02, GLfloat _03, 
			   GLfloat _10, GLfloat _11, GLfloat _12, GLfloat _13, 
			   GLfloat _20, GLfloat _21, GLfloat _22, GLfloat _23, 
			   GLfloat _30, GLfloat _31, GLfloat _32, GLfloat _33)
{
	data[0]=_00;	data[1]=_01;	data[2]=_02;	data[3]=_03;
	data[4]=_10;	data[5]=_11;	data[6]=_12;	data[7]=_13;
	data[8]=_20;	data[9]=_21;	data[10]=_22;	data[11]=_23;
	data[12]=_30;	data[13]=_31;	data[14]=_32;	data[15]=_33;
}


/*
 *	make this matrix identity
 */
void Matrix::Identity()
{
	memset(data, 0, 16*sizeof(GLfloat));
	data[0] = data[5] = data[10] = data[15] = 1.0f;
}


/*
 * just set all the elements of the matrix...	
 */
void Matrix::Set(GLfloat _00, GLfloat _01, GLfloat _02, GLfloat _03, 
			GLfloat _10, GLfloat _11, GLfloat _12, GLfloat _13, 
			GLfloat _20, GLfloat _21, GLfloat _22, GLfloat _23, 
			GLfloat _30, GLfloat _31, GLfloat _32, GLfloat _33)
{
	data[0]=_00;	data[1]=_01;	data[2]=_02;	data[3]=_03;
	data[4]=_10;	data[5]=_11;	data[6]=_12;	data[7]=_13;
	data[8]=_20;	data[9]=_21;	data[10]=_22;	data[11]=_23;
	data[12]=_30;	data[13]=_31;	data[14]=_32;	data[15]=_33;
}

/*
 *	set the matrix elements from an float[16]
 */
void Matrix::Set(GLfloat mat[16])
{
	memcpy(data,mat,sizeof(GLfloat)*16);
}


/*
 *	transpose the matrix
 */
void Matrix::Transpose()
{
	Matrix tmp(data[0],data[4],data[8],data[12],
		data[1],data[5],data[9],data[13],
		data[2],data[6],data[10],data[14],
		data[3],data[7],data[11],data[15]);
	(*this) = tmp;
}


/*
 *	inverse the matrix
 */
void Matrix::Inverse()
{
	float tmpmatrix[16] = {0};
	float d12, d13, d23, d24, d34, d41;

	d12 = data[2]  * data[7]  - data[3]  * data[6];
	d13 = data[2]  * data[11] - data[3]  * data[10];
	d23 = data[6]  * data[11] - data[7]  * data[10];
	d24 = data[6]  * data[15] - data[7]  * data[14];
	d34 = data[10] * data[15] - data[11] * data[14];
	d41 = data[14] * data[3]  - data[15] * data[2];

	tmpmatrix[0] =   data[5] * d34 - data[9] * d24 + data[13] * d23;
	tmpmatrix[1] = -(data[1] * d34 + data[9] * d41 + data[13] * d13);
	tmpmatrix[2] =   data[1] * d24 + data[5] * d41 + data[13] * d12;
	tmpmatrix[3] = -(data[1] * d23 - data[5] * d13 + data[9]  * d12);

	// Calculate the determinant.
	float determinant = data[0]*tmpmatrix[0] + data[4]*tmpmatrix[1] +
		data[8]*tmpmatrix[2] + data[12]*tmpmatrix[3];


	// Clear if the determinant is equal to zero.  
	// 0 means matrix have no inverse.
	if(determinant == 0.0f)
	{
		Identity();
		return;
	}


	float invDeterminant = 1.0f / determinant;

	// Compute rest of inverse.
	tmpmatrix[0] *= invDeterminant;
	tmpmatrix[1] *= invDeterminant;
	tmpmatrix[2] *= invDeterminant;
	tmpmatrix[3] *= invDeterminant;

	tmpmatrix[4] = -(data[4] * d34 - data[8] * d24 + data[12] * d23) * invDeterminant;
	tmpmatrix[5] =   data[0] * d34 + data[8] * d41 + data[12] * d13  * invDeterminant;
	tmpmatrix[6] = -(data[0] * d24 + data[4] * d41 + data[12] * d12) * invDeterminant;
	tmpmatrix[7] =   data[0] * d23 - data[4] * d13 + data[8]  * d12  * invDeterminant;

	// Pre-compute 2x2 dets for first two rows when computing cofactors 
	// of last two rows.
	d12 = data[0]  * data[5]  - data[1]  * data[12];
	d13 = data[0]  * data[9]  - data[1]  * data[8];
	d23 = data[4]  * data[9]  - data[5]  * data[8];
	d24 = data[4]  * data[13] - data[5]  * data[12];
	d34 = data[8]  * data[13] - data[9]  * data[12];
	d41 = data[12] * data[1]  - data[13] * data[0];

	tmpmatrix[8]  =   data[7] * d34 - data[11] * d24 + data[15] * d23 * invDeterminant;
	tmpmatrix[9]  = -(data[3] * d34 + data[11] * d41 + data[15] * d13) * invDeterminant;
	tmpmatrix[10] =   data[3] * d24 + data[7]  * d41 + data[15] * d12 * invDeterminant;
	tmpmatrix[11] = -(data[3] * d23 - data[7]  * d13 + data[11] * d12) * invDeterminant;
	tmpmatrix[12] = -(data[6] * d34 - data[10] * d24 + data[14] * d23) * invDeterminant;
	tmpmatrix[13] =   data[2] * d34 + data[10] * d41 + data[14] * d13 * invDeterminant;
	tmpmatrix[14] = -(data[2] * d24 + data[6]  * d41 + data[14] * d12) * invDeterminant;
	tmpmatrix[15] =   data[2] * d23 - data[6]  * d13 + data[10] * d12 * invDeterminant;

	// Save the temp matrix to our matrix.
	Set(tmpmatrix);
}


/*
 *	return inversed copy of the matrix
 *	seems to be more accurate than the one above
 */
Matrix Matrix::Inversed() const
{
	Matrix M;
	const float M3344(data[10]*data[15] - data[11]*data[14]);
	const float M3244(data[9] *data[15] - data[11]*data[13]);
	const float M3243(data[9] *data[14] - data[10]*data[13]); 
	const float M2344(data[6] *data[15] - data[7] *data[14]);
	const float M2244(data[5] *data[15] - data[7] *data[13]);
	const float M2243(data[5] *data[14] - data[6] *data[13]);
	const float M2334(data[6] *data[11] - data[7] *data[10]);
	const float M2234(data[5] *data[11] - data[7] *data[9]);
	const float M2233(data[5] *data[10] - data[6] *data[9]);
	const float M3144(data[8] *data[15] - data[11]*data[12]);
	const float M3143(data[8] *data[14] - data[10]*data[12]);
	const float M2144(data[4] *data[15] - data[7] *data[12]);
	const float M2143(data[4] *data[14] - data[6] *data[12]);
	const float M2134(data[4] *data[11] - data[7] *data[8]);
	const float M2133(data[4] *data[10] - data[6] *data[8]);
	const float M3142(data[8] *data[13] - data[9] *data[12]);
	const float M2142(data[4] *data[13] - data[5] *data[12]);
	const float M2132(data[4] *data[9]  - data[5] *data[8]);

	M[0] =  data[5] * M3344 - data[6] * M3244 + data[7] * M3243;
	M[1] = -data[1] * M3344 + data[2] * M3244 - data[3] * M3243;
	M[2] =  data[1] * M2344 - data[2] * M2244 + data[3] * M2243;
	M[3] = -data[1] * M2334 + data[2] * M2234 - data[3] * M2233;

	M[4] = -data[4] * M3344 + data[6] * M3144 - data[7] * M3143;
	M[5] =  data[0] * M3344 - data[2] * M3144 + data[3] * M3143;
	M[6] = -data[0] * M2344 + data[2] * M2144 - data[3] * M2143;
	M[7] =  data[0] * M2334 - data[2] * M2134 + data[3] * M2133;

	M[8] =  data[4] * M3244 - data[5] * M3144 + data[7] * M3142;
	M[9] = -data[0] * M3244 + data[1] * M3144 - data[3] * M3142;
	M[10]=  data[0] * M2244 - data[1] * M2144 + data[3] * M2142;
	M[11]= -data[0] * M2234 + data[1] * M2134 - data[3] * M2132;

	M[12]= -data[4] * M3243 + data[5] * M3143 - data[6] * M3142;
	M[13]=  data[0] * M3243 - data[1] * M3143 + data[2] * M3142;
	M[14]= -data[0] * M2243 + data[1] * M2143 - data[2] * M2142;
	M[15]=  data[0] * M2233 - data[1] * M2133 + data[2] * M2132;

	// divide with the determinant
	M /= data[0]*M[0] + data[1]*M[4] + data[2]*M[8] + data[3]*M[12];   

	return M;
}

/*
 *	returns the result of v*Matrix operation
 */
Vector3 Matrix::VectorMultiply(const Vector3& v) const
{
	Vector3 tmp( v.x*data[0] + v.y*data[4] + v.z*data[8] + data[12],
				 v.x*data[1] + v.y*data[5] + v.z*data[9] + data[13],
				 v.x*data[2] + v.y*data[6] + v.z*data[10] + data[14]);
	return tmp;
}

/*
 *	apply the transformation (rotation and translation)described by the matrix to v.
 *	basically, multiply v with matrix and store product to v
 */
void Matrix::VectorTransform(Vector3& v) const
{
	Vector3 tmp(v);
	v.x = tmp.x*data[0] + tmp.y*data[4] + tmp.z*data[8] + data[12];
	v.y = tmp.x*data[1] + tmp.y*data[5] + tmp.z*data[9] + data[13];
	v.z = tmp.x*data[2] + tmp.y*data[6] + tmp.z*data[10] + data[14];
}

/*
 *	apply only the rotation...
 */
void Matrix::VectorRotate(Vector3& v) const
{
	Vector3 tmp(v);
	v.x = tmp.x*data[0] + tmp.y*data[4] + tmp.z*data[8];
	v.y = tmp.x*data[1] + tmp.y*data[5] + tmp.z*data[9];
	v.z = tmp.x*data[2] + tmp.y*data[6] + tmp.z*data[10];
}

/*
 *	apply inverse rotation
 */
void Matrix::VectorInvRotate(Vector3& v) const
{
	Vector3 tmp(v);
	v.x = tmp.x*data[0] + tmp.y*data[1] + tmp.z*data[2];
	v.y = tmp.x*data[4] + tmp.y*data[5] + tmp.z*data[6];
	v.z = tmp.x*data[8] + tmp.y*data[9] + tmp.z*data[10];
}

/*
 *	apply only the translation
 */
void Matrix::VectorTranslate(Vector3& v) const
{
	v.x += data[12];
	v.y += data[13];
	v.z += data[14];
}

/*
 *	apply inverse translation
 */
void Matrix::VectorInvTranslate(Vector3& v) const
{
	v.x -= data[12];
	v.y -= data[13];
	v.z -= data[14];
}


/*
 *	create a scale matrix
 */
void Matrix::SetScale(float x,float y,float z)
{
	Identity();
	data[0] = x;
	data[5] = y;
	data[10]= z;
}

/*
 *	create a translation matrix
 */
void Matrix::SetTranslation(float x,float y,float z)
{
	Identity();
	data[12] = x;
	data[13] = y;
	data[14] = z;
}

/*
 *	create a rotation matrix from an angle and 3 axis
 *	angle in radians
 */
void Matrix::SetRotation(double angle,float x,float y,float z)
{
	if(x==0 && y==0 && z==0)
	{
		Identity();
		return;
	}
	
	float length = (float)sqrt(x*x+y*y+z*z);

	x /= length;
	y /= length;
	z /= length;

	float sina = (float)sin(angle);
	float cosa = (float)cos(angle);
	float oneMinusCos = 1.0f - cosa;

	data[0] = (x*x*oneMinusCos) + cosa;
	data[4] = (x*y*oneMinusCos) - z*sina;
	data[8] = (x*z*oneMinusCos) + y*sina;
	data[12] = 0.0f;

	data[1] = (y*z*oneMinusCos) + z*sina;
	data[5] = (y*y*oneMinusCos) + cosa;
	data[9] = (y*z*oneMinusCos) - x*sina;
	data[13] = 0.0f;

	data[2] = (z*x*oneMinusCos) - y*sina;
	data[6] = (z*y*oneMinusCos) + x*sina;
	data[10]= (z*z*oneMinusCos)	+ cosa;
	data[14] = 0.0f;

	data[3] = 0.0f;
	data[7] = 0.0f;
	data[11]= 0.0f;
	data[15]= 1.0f;
}


/*
 *	create a rotation matrix from 3 angles
 *	angle in radians
 */
void Matrix::SetRotation(double xangle,double yangle, double zangle)
{
	float cx = (float)cos( xangle );
	float sx = (float)sin( xangle );
	float cy = (float)cos( yangle );
	float sy = (float)sin( yangle );
	float cz = (float)cos( zangle );
	float sz = (float)sin( zangle );

	data[0] = cy*cz;
	data[1] = cy*sz;
	data[2] = -sy;
	data[3] = 0.0f;

	float sxsy = sx*sy;
	float cxsy = cx*sy;

	data[4] = sxsy*cz-cx*sz;
	data[5] = sxsy*sz+cx*cz;
	data[6] = sx*cy;
	data[7] = 0.0f;

	data[8] = cxsy*cz+sx*sz;
	data[9] = cxsy*sz-sx*cz;
	data[10] = cx*cy;
	data[11] = 0.0f;

	data[12] = 0.0f;
	data[13] = 0.0f;
	data[14] = 0.0f;
	data[15] = 1.0f;
}

void Matrix::SetInvRotation(double xangle,double yangle, double zangle)
{
	float cx = (float)cos( xangle );
	float sx = (float)sin( xangle );
	float cy = (float)cos( yangle );
	float sy = (float)sin( yangle );
	float cz = (float)cos( zangle );
	float sz = (float)sin( zangle );

	data[0] = cy*cz;
	data[4] = cy*sz;
	data[8] = -sy;
	data[12] = 0.0f;

	float sxsy = sx*sy;
	float cxsy = cx*sy;

	data[1] = sxsy*cz-cx*sz;
	data[5] = sxsy*sz+cx*cz;
	data[9] = sx*cy;
	data[13] = 0.0f;

	data[2] = cxsy*cz+sx*sz;
	data[6] = cxsy*sz-sx*cz;
	data[10] = cx*cy;
	data[14] = 0.0f;

	data[3] = 0.0f;
	data[7] = 0.0f;
	data[11] = 0.0f;
	data[15] = 1.0f;
}


/*
 *	create the matrix from a quaternion
 */
void Matrix::SetFromQuaternion(Quaternion& quat)
{
	quat.CreateMatrix(data,true);
}



/*
 *	create shadow matrix for a plane that intersects with (0,0,0)
 *	i.e. that has zero distance from the center of the coordinate axis
 *	it may have any orientation though
 */
void Matrix::CreateShadowMatrix(const Vector3& planeNormal,const Vector3& lightPos)
{
	Identity();

	//
	// To create a shadow matrix we first need the dot product of the ground 
	// normal and the light position.  We store the result in a float called 
	// dotProduct.  
	float dotProduct = Dot3(planeNormal,lightPos);

	//
	// Create the shadow matrix by adding our values like so...
	data[0]  = dotProduct - lightPos.x * planeNormal.x;
	data[4]  = 0.0f       - lightPos.x * planeNormal.y;
	data[8]  = 0.0f       - lightPos.x * planeNormal.z;
	data[12] = 0.0f       - lightPos.x;

	data[1]  = 0.0f       - lightPos.y * planeNormal.x;
	data[5]  = dotProduct - lightPos.y * planeNormal.y;
	data[9]  = 0.0f       - lightPos.y * planeNormal.z;
	data[13] = 0.0f       - lightPos.y;

	data[2]  = 0.0f       - lightPos.z * planeNormal.x;
	data[6]  = 0.0f       - lightPos.z * planeNormal.y;
	data[10] = dotProduct - lightPos.z * planeNormal.z;
	data[14] = 0.0f       - lightPos.z;

	data[3]  = 0.0f       - planeNormal.x;
	data[7]  = 0.0f       - planeNormal.y;
	data[11] = 0.0f       - planeNormal.z;
	data[15] = dotProduct - 1.0f;

	//
	// Notice that the dotProuct is being subtracted from the light position and 
	// the plane normal diagonally. You will also notice that the x light position 
	// is used in the first column, y for the second, and so on.  This is the math 
	// to create a shadow projection matrix.  I wouldn't both taking out a pencil 
	// and paper and tracing this on out.  Just know that this is how you set it up.	
}


/*
 *	create a shadow matrix that will project shadows to the plane described by 
 *	3 points
 */
void Matrix::CreateShadowMatrix(const Vector3 points[3],const Vector3& lightPos)
{
	Plane plane(points[0],points[1],points[2]);

	float dot = plane.Distance(lightPos);

	const Vector3& norm = plane.m_normal;
	float inter = plane.m_intercept;

	// Now do the projection
	// First column
	data[0]	= dot	- lightPos[0] * norm[0];
	data[4] = 0.0f	- lightPos[0] * norm[1];
	data[8] = 0.0f	- lightPos[0] * norm[2];
	data[12]= 0.0f	- lightPos[0] * inter;

	// Second column
	data[1] = 0.0f	- lightPos[1] * norm[0];
	data[5] = dot	- lightPos[1] * norm[1];
	data[9] = 0.0f	- lightPos[1] * norm[2];
	data[13]= 0.0f	- lightPos[1] * inter;

	// Third Column
	data[2] = 0.0f	- lightPos[2] * norm[0];
	data[6] = 0.0f	- lightPos[2] * norm[1];
	data[10]= dot	- lightPos[2] * norm[2];
	data[14]= 0.0f	- lightPos[2] * inter;

	// Fourth Column
	data[3] = 0.0f	- lightPos[3] * norm[0];
	data[7] = 0.0f	- lightPos[3] * norm[1];
	data[11]= 0.0f	- lightPos[3] * norm[2];
	data[15]= dot	- lightPos[3] * inter;

}

/*
 * 	
 */
Matrix Matrix::operator-(const Matrix& m) const
{
	return Matrix(
		data[0]-m[0],  data[1]-m[1],  data[2]-m[2],  data[3]-m[3], 
		data[4]-m[4],  data[5]-m[5],  data[6]-m[6],  data[7]-m[7],
		data[8]-m[8],  data[9]-m[9],  data[10]-m[10],data[11]-m[11],
		data[12]-m[12],data[13]-m[13],data[14]-m[14],data[15]-m[15]
	);
}

Matrix Matrix::operator+(const Matrix& m) const
{
	return Matrix(
		data[0]+m[0],  data[1]+m[1],  data[2]+m[2],  data[3]+m[3], 
		data[4]+m[4],  data[5]+m[5],  data[6]+m[6],  data[7]+m[7],
		data[8]+m[8],  data[9]+m[9],  data[10]+m[10],data[11]+m[11],
		data[12]+m[12],data[13]+m[13],data[14]+m[14],data[15]+m[15]
		);
}

Matrix Matrix::operator*(const Matrix& m) const
{
	Matrix product;
	const float *m1 = data, *m2 = m.data;
	float *m3 = product.data;

	m3[0]	= m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2];
	m3[1]	= m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2];
	m3[2]	= m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2];
	m3[3]	= 0.0f;

	m3[4]	= m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6];
	m3[5]	= m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6];
	m3[6]	= m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6];
	m3[7]	= 0.0f;

	m3[8]	= m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10];
	m3[9]	= m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10];
	m3[10]	= m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10];
	m3[11]	= 0.0f;

	m3[12]	= m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12];
	m3[13]	= m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13];
	m3[14]	= m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14]+ m1[14];
	m3[15]	= 1.0f;

	return product;
}


Matrix Matrix::operator/(const Matrix& m) const
{
	const float *m1 = data, *m2=m.data;

	return Matrix(
		m1[0]/m2[0] + m1[4]/m2[1] + m1[8]/m2[2] + m1[12]/m1[3],
		m1[1]/m2[0] + m1[5]/m2[1] + m1[9]/m2[2] + m1[13]/m2[3],
		m1[2]/m2[0] + m1[6]/m2[1] + m1[10]/m2[2] + m1[14]/m2[3],
		m1[3]/m2[0] + m1[7]/m2[1] + m1[11]/m2[2] + m1[15]/m2[3],
		m1[0]/m2[4] + m1[4]/m2[5] + m1[8]/m2[6] + m1[12]/m2[7],
		m1[1]/m2[4] + m1[5]/m2[5] + m1[9]/m2[6] + m1[13]/m2[7],
		m1[2]/m2[4] + m1[6]/m2[5] + m1[10]/m2[6] + m1[14]/m2[7],
		m1[3]/m2[4] + m1[7]/m2[5] + m1[11]/m2[6] + m1[15]/m2[7],
		m1[0]/m2[8] + m1[4]/m2[9] + m1[8]/m2[10] + m1[12]/m2[11],
		m1[1]/m2[8] + m1[5]/m2[9] + m1[9]/m2[10] + m1[13]/m2[11],
		m1[2]/m2[8] + m1[6]/m2[9] + m1[10]/m2[10] + m1[14]/m2[11],
		m1[3]/m2[8] + m1[7]/m2[9] + m1[11]/m2[10] + m1[15]/m2[11],
		m1[0]/m2[12] + m1[4]/m2[13] + m1[8]/m2[14] + m1[12]/m2[15],
		m1[1]/m2[12] + m1[5]/m2[13] + m1[9]/m2[14] + m1[13]/m2[15],
		m1[2]/m2[12] + m1[6]/m2[13] + m1[10]/m2[14] + m1[14]/m2[15],
		m1[3]/m2[12] + m1[7]/m2[13] + m1[11]/m2[14] + m1[15]/m2[15]
		);
}


void Matrix::operator-=(const Matrix& m)
{
	for(int i=0; i<16; i++)
		data[i] -= m.data[i];
}

void Matrix::operator+=(const Matrix& m)
{
	for(int i=0; i<16; i++)
		data[i] += m.data[i];
}

void Matrix::operator *=(const Matrix& m)
{
	(*this) = (*this)*m;
}

void Matrix::operator /=(const Matrix& m)
{
	(*this) = (*this)/m;
}

Matrix Matrix::operator * (const float& f) const
{
	return Matrix(
		data[0]*f,	data[1]*f,	data[2]*f,	data[3]*f,
		data[4]*f,	data[5]*f,	data[6]*f,	data[7]*f,
		data[8]*f,	data[9]*f,	data[10]*f,	data[11]*f,
		data[12]*f,	data[13]*f,	data[14]*f,	data[15]*f
		);
}

Matrix Matrix::operator / (const float& f) const
{
	return Matrix(
		data[0]/f,	data[1]/f,	data[2]/f,	data[3]/f,
		data[4]/f,	data[5]/f,	data[6]/f,	data[7]/f,
		data[8]/f,	data[9]/f,	data[10]/f,	data[11]/f,
		data[12]/f,	data[13]/f,	data[14]/f,	data[15]/f
		);
}

Matrix Matrix::operator + (const float& f) const
{
	return Matrix(
		data[0]+f,	data[1]+f,	data[2]+f,	data[3]+f,
		data[4]+f,	data[5]+f,	data[6]+f,	data[7]+f,
		data[8]+f,	data[9]+f,	data[10]+f,	data[11]+f,
		data[12]+f,	data[13]+f,	data[14]+f,	data[15]+f
		);
}

Matrix Matrix::operator - (const float& f) const
{
	return Matrix(
		data[0]-f,	data[1]-f,	data[2]-f,	data[3]-f,
		data[4]-f,	data[5]-f,	data[6]-f,	data[7]-f,
		data[8]-f,	data[9]-f,	data[10]-f,	data[11]-f,
		data[12]-f,	data[13]-f,	data[14]-f,	data[15]-f
		);
}


void Matrix::operator += (const float& f)
{
	for (int i=0; i<16; i++)
		data[i] += f;
}

void Matrix::operator -= (const float& f)
{
	for (int i=0; i<16; i++)
		data[i] -= f;
}


void Matrix::operator *= (const float& f)
{
	for (int i=0; i<16; i++)
		data[i] *= f;
}

void Matrix::operator /= (const float& f)
{
	for (int i=0; i<16; i++)
		data[i] /= f;
}


/*
 *	check for matrix equality
 *  return false if at least one element is different
 */
bool Matrix::operator == (const Matrix& m) const
{
	for(int i=0; i<16; i++)
	{
		if(data[i] != m.data[i])
			return false;
	}
	return true;
}


bool Matrix::operator != (const Matrix& m) const
{
	return !((*this)==m);
}


/*
 *	return the product of v*m
 *	actually, is the same as Matrix::VectorMultiply
 *  
 */
Vector3 glmath::operator*(const Vector3& v,const Matrix& m)
{
	const float* data = m.data;

	return Vector3(	
		data[0]*v[0]  + data[4]*v[1]  + data[8]* v[2]  + data[12],
		data[1]*v[0]  + data[5]*v[1]  + data[9]* v[2]  + data[13],
		data[2]*v[0]  + data[6]*v[1]  + data[10]* v[2]  + data[14]
		);
}


void glmath::glLoadMatrix(const Matrix& m)
{
	 ::glLoadMatrixf(m.data);
}

void glmath::glMultMatrix(const Matrix& m)
{ 
	::glMultMatrixf(m.data); 
}

void glmath::glGetProjection(Matrix& m)
{ 
	::glGetFloatv(GL_PROJECTION_MATRIX,m.data); 	
}

void glmath::glGetModelview(Matrix& m)
{ 
	::glGetFloatv(GL_MODELVIEW_MATRIX,m.data); 	
}



/************************************************************************/
/* Plane class
*/
/************************************************************************/


/*
 *	create plane given 3 of its points
 */
void Plane::SetFromPoints(const Vector3& v1,const Vector3& v2,const Vector3& v3)
{
	Vector3 tmp1(v2-v1);
	Vector3 tmp2(v3-v2);
	m_normal = Cross3(tmp1,tmp2).Normalized();
	
	CalcIntercept(v1);
}


/*
 *	normalize plane
 */
void Plane::Normalize()
{
	float length = m_normal.Length();
	m_normal /= length;
	//m_intercept /= length;
}

/*
 *	find point of intersection of 3 planes
 */
bool Plane::Intersect3(const Plane& p2,const Plane& p3,Vector3& vresult)
{
	float denominator = 
		m_normal.DotProduct((p2.GetNormal()).CrossProduct(p3.GetNormal()));

	if (denominator==0.0f)
		return false;	//no intersection

	Vector3 v1( p2.GetNormal().CrossProduct(p3.GetNormal())*m_intercept);
	Vector3 v2( p3.GetNormal().CrossProduct(GetNormal())*m_intercept);
	Vector3 v3( GetNormal().CrossProduct(p2.GetNormal())*m_intercept);

	vresult = (v1+v2+v3)/(-denominator);

	return true;
}


/*
 *	distance between vector v and the plane
 */
float Plane::Distance(const Vector3& v) const
{
	return v.x*m_normal.x + v.y*m_normal.y + v.z*m_normal.z + m_intercept;
}

/*
 *	where is vector in plane's space?
 */
int Plane::ClassifyPoint(const Vector3& v) const
{
	float dist = Distance(v);

	if(dist > (float)SMALL_REAL)
		return Plane::POINT_IN_FRONT_OF_PLANE;
	if(dist < (float)-SMALL_REAL)
		return Plane::POINT_BEHIND_PLANE;
	return Plane::POINT_ON_PLANE;
}


/*
 *	return linear interpolation of this plane and plane p2 by factor
 */
Plane Plane::Lerp(const Plane& p2,float factor)
{
	Plane result;

	result.SetNormal( (m_normal*(1.0f-factor) + p2.GetNormal()*factor).Normalized() );
	result.SetIntercept( m_intercept*(1.0f-factor) + p2.GetIntercept()*factor);

	return result;
}


/*
 *	check for equality between planes
 */
bool Plane::operator ==(const Plane& p2) const
{
	return (m_normal==p2.GetNormal() && CompareTolerant(m_intercept,p2.GetIntercept()));
}





/************************************************************************/
/*	Frustum class for culling  

*/
/************************************************************************/


/*
 *	set the planes of the frustum based on the 
 *	current OpenGL projection and modelview matrices
 */
void Frustum::Update()
{
	Matrix mprojection,mview,mclip;

	//
	// get matrices
	glGetProjection(mprojection);
	glGetModelview(mview);
	
	mclip = mprojection*mview;

	//
	// calculate planes
	
	m_planes[RIGHT_PLANE].SetNormal( 
		Vector3(mclip[3]-mclip[0],mclip[7]-mclip[4],mclip[11]-mclip[8]) );
	m_planes[RIGHT_PLANE].SetIntercept(mclip[15]-mclip[12]);

	m_planes[LEFT_PLANE].SetNormal( 
		Vector3(mclip[3]+mclip[0],mclip[7]+mclip[4],mclip[11]+mclip[8] ));
	m_planes[LEFT_PLANE].SetIntercept(mclip[15]+mclip[12] );

	m_planes[LEFT_PLANE].SetNormal(
		Vector3(mclip[3]+mclip[1],mclip[7]+mclip[5],mclip[11]+mclip[9] ));
	m_planes[LEFT_PLANE].SetIntercept(mclip[15]+mclip[13] );

	m_planes[LEFT_PLANE].SetNormal(
		Vector3(mclip[3]-mclip[1],mclip[7]-mclip[5],mclip[11]-mclip[9] ));
	m_planes[LEFT_PLANE].SetIntercept( mclip[15]-mclip[13] );

	m_planes[LEFT_PLANE].SetNormal(
		Vector3(mclip[3]-mclip[2],mclip[7]-mclip[6],mclip[11]-mclip[10] ));
	m_planes[LEFT_PLANE].SetIntercept( mclip[15]-mclip[14] );

	m_planes[LEFT_PLANE].SetNormal( 
		Vector3(mclip[3]+mclip[2],mclip[7]+mclip[6],mclip[11]+mclip[10] ));
	m_planes[LEFT_PLANE].SetIntercept( mclip[15]+mclip[14] );

	//
	// normalize planes
	for(int i=0; i<6; ++i)
		m_planes[i].Normalize();
}


/*
 *	check if a point is inside frustum
 *	if one of the frustum planes determines that the point is behind it, then
 *	the point is outside frustum
 */
bool Frustum::IsPointInside(const Vector3& point) const
{
	//
	//loop through planes
	for(int i=0; i<6; ++i)
	{
		if(m_planes[i].ClassifyPoint( point )==Plane::POINT_BEHIND_PLANE)
			return false;
	}
	return true;
}


/*
 *	check if a bounding box is inside the frustum
 *	at least one vertex of the box must be inside the frustum
 */
bool Frustum::IsBoundingBoxInside(const Vector3 vertices[8]) const
{
	//
	//loop through planes
	for(int i=0; i<6; ++i)
	{
		//if a point is not behind this plane, try next plane
		if(m_planes[i].ClassifyPoint(vertices[0])!=Plane::POINT_BEHIND_PLANE)
			continue;
		if(m_planes[i].ClassifyPoint(vertices[1])!=Plane::POINT_BEHIND_PLANE)
			continue;
		if(m_planes[i].ClassifyPoint(vertices[2])!=Plane::POINT_BEHIND_PLANE)
			continue;
		if(m_planes[i].ClassifyPoint(vertices[3])!=Plane::POINT_BEHIND_PLANE)
			continue;
		if(m_planes[i].ClassifyPoint(vertices[4])!=Plane::POINT_BEHIND_PLANE)
			continue;
		if(m_planes[i].ClassifyPoint(vertices[5])!=Plane::POINT_BEHIND_PLANE)
			continue;
		if(m_planes[i].ClassifyPoint(vertices[6])!=Plane::POINT_BEHIND_PLANE)
			continue;
		if(m_planes[i].ClassifyPoint(vertices[7])!=Plane::POINT_BEHIND_PLANE)
			continue;

		//All vertices of the box are behind this plane
		return false;
	}
	return true;
}

/*
 *	for a bounding sphere, to be inside the frustum (not necessarily entirely)
 *	the distance between it's center and every plane of the frustum must be  > -radius
 */
bool Frustum::IsBoundingSphereInside(const Vector3& center,float radius) const
{
	//
	// loop through planes
	for (int i=0; i<6; i++)
	{
		if(m_planes[i].Distance(center) <= -radius)
			return false;
	}
	return true;
}

bool Frustum::IsBoundingSphereInside(const BoundingSphere& bs) const
{
	return IsBoundingSphereInside(bs.m_center,bs.m_radius);
}



/************************************************************************/
/*	Bitset class: set of bits to represent many true/falses  

*/
/************************************************************************/


bool Bitset::Init(int numberOfBits)
{
	//
	// Delete any memory allocated to bits	
	delete[] m_pbits;
	m_pbits=NULL;

	m_numbytes = (numberOfBits>>3)+1;

	m_pbits = new unsigned char[m_numbytes];
	if (!m_pbits)
	{
		Log("cBitset::init: Unable to allocate space for "\
			"a bitset of %d bits", numberOfBits);
		return false;
	}

	ClearAll();
	return true;
}


void Bitset::ClearAll()
{
	if(m_pbits)
		memset(m_pbits, 0, m_numbytes);	
}

void Bitset::SetAll()
{
	if(m_pbits)
		memset(m_pbits, 0xFF, m_numbytes);
}

void Bitset::Clear(int bitNumber)
{
	m_pbits[bitNumber>>3] &= ~(1<<(bitNumber & 7));
}

void Bitset::Set(int bitNumber)
{
	m_pbits[bitNumber>>3] |= ~(1<<(bitNumber & 7));
}

unsigned char Bitset::IsSet(int bitNumber)
{
	return m_pbits[bitNumber>>3] & 1<<(bitNumber&7);
}



/************************************************************************/
/*	BezierCurve															*/
/*																		*/
/*																		*/
/************************************************************************/

BezierCurve::BezierCurve(Vector3& p1,Vector3& p2,Vector3& p3,Vector3& p4)
:	pstart(p1),pcontrol1(p2),pcontrol2(p3),pend(p4)
{}

/*
 *	find the interpolated position on the curve by factor t
 *
 */
Vector3 BezierCurve::PointOnCurve(const float t) const
{	
	Vector3 point;
	float var1,var2,var3,var4;

	var1 = 1-t;
	var2 = var1*var1;
	var3 = var1*var1*var1;
	var4 = t*t*t;

	point = pstart*var3 + pcontrol1*3*t*var2 + pcontrol2*3*t*t*var1 + pend*var4;

	return point;
}

/*
 *	just set the parameters of the curve:
 *	
 */
void BezierCurve::Set(const Vector3& p1,const Vector3& p2,const Vector3& p3,
					  const Vector3& p4)
{
	pstart=p1;	pcontrol1=p2;	pcontrol2=p3;	pend=p4;
}


/************************************************************************/
/* bounding sphere class                                                */
/*																		*/
/*																		*/
/*																		*/
/************************************************************************/

/*
 *	if the distance of the centers of the 2 spheres is less or equal of the sum of 
 *	their radius, then they collide
 */
bool BoundingSphere::Collides(const BoundingSphere& bs) const
{
	float dist_between_centers = Distance(m_center,bs.m_center);
	float sum_of_radius = m_radius+bs.m_radius;
	if(dist_between_centers<sum_of_radius)
		return true;
	return false;
}


/*
 *	if the distance between the point and the center of the sphere is less than 
 *	the sphere's radius, then the point is inside the sphere
 */
bool BoundingSphere::IsPointInside(const Vector3& v) const
{
	float dist = Distance(m_center,v);
	if(dist<m_radius)
		return true;
	return false;
}

/*
 *	determine where the point lies in reference to the sphere
 */
BoundingSphere::enumPointClassification 
BoundingSphere::ClassifyPoint(const Vector3& v) const
{
	float dist = Distance(m_center,v);

	if(dist<m_radius)
		return POINT_INSIDE_SPHERE;
	else if(dist==m_radius)
		return POINT_ON_SPHERE;
	else
		return POINT_OUTSIDE_SPHERE;
}


/************************************************************************/
/* bounding box class		                                            */
/*																		*/
/*																		*/
/*																		*/
/************************************************************************/


bool BoundingBox::Collides(const BoundingBox& bb) const
{
	Vector3 diff(m_center - bb.m_center);	// the distance of the 2 centers

	if ( abs(diff.x) <= (m_xdim+bb.m_xdim) ||
		 abs(diff.y) <= (m_ydim+bb.m_ydim) ||
		 abs(diff.z) <= (m_zdim+bb.m_zdim))
	{
		return true;
	}
	return false;
}

bool BoundingBox::IsPointinside(const Vector3& v) const
{
	Vector3 diff(v - m_center);

	return	(abs(diff.x)<m_xdim) && 
			(abs(diff.y)<m_ydim) && 
			(abs(diff.z)<m_zdim);
}

BoundingBox::enumPointClassification
BoundingBox::ClassifyPoint(const Vector3& v) const
{
	Vector3 diff(v-m_center);
	
	
	if (abs(diff.x)==m_xdim)
	{
		if((abs(diff.y)<=m_ydim) && (abs(diff.z)<=m_zdim))
			return POINT_ON_BOX;
	}
	else if (abs(diff.y)==m_ydim)
	{
		if((abs(diff.x)<=m_xdim) && (abs(diff.z)<=m_zdim))
			return POINT_ON_BOX;
	}
	else if (abs(diff.z)==m_zdim)
	{
		if((abs(diff.x)<=m_xdim) && (abs(diff.y)<=m_ydim))
			return POINT_ON_BOX;
	}
	else if ( abs(diff.x)<m_xdim && (abs(diff.y)<m_ydim) && (abs(diff.z)<m_zdim))
	{
		return POINT_INSIDE_BOX;
	}
	
	
	return POINT_OUTSIDE_BOX;	
}

//
// if point v is outside of the box, check where exactly is
bool BoundingBox::LocatePoint(const Vector3& v, enumPointClassification where) const
{
	if(IsPointinside(v))
		return false;

	Vector3 diff(v-m_center);

	switch (where)
	{
	case POINT_IN_FRONT_OF_BOX:
		return (diff.z > 0);
	case POINT_BACK_OF_BOX:
		return (diff.z < 0);
	case POINT_LEFT_OF_BOX:
		return (diff.x < 0);
	case POINT_RIGHT_OF_BOX:
		return (diff.x > 0);
	case POINT_ABOVE_BOX:
		return (diff.y > 0);
	case POINT_BELLOW_BOX:
		return (diff.y < 0);
	}
	
	return false;	// possibly wrong argument....
}

std::pair<bool, float> glmath::Ray::Intersects( const Plane& p ) const
{
	float denom = p.GetNormal().DotProduct(this->GetDirection());

	if (fabs(denom)<std::numeric_limits<float>::epsilon())
	{
		//parallel
		return std::pair<bool,float>(false,0);
	}
	else
	{
		float nom = p.GetNormal().DotProduct(this->GetOrigin()+p.GetIntercept());
		float t = -(nom/denom);
		return std::pair<bool,float>(t>=0,t);
	}
}

std::pair<bool, float> glmath::Ray::Intersects( const BoundingSphere& sphere ) const
{
	const Vector3& raydir = this->GetDirection();
	// Adjust ray origin relative to sphere center
	const Vector3& rayorig = this->GetOrigin() - sphere.GetCenter();
	float radius = sphere.GetRadius();

	// Check if origin inside first
	if (sphere.IsPointInside(rayorig))
	{
		return std::pair<bool,float>(true,0);
	}

	//
	// quadratics
	// build coefficients, for std quadratic solver
	// ie t = (-b +/- sqrt(b*b + 4ac)) / 2a
	float a = raydir.DotProduct(raydir);
	float b = 2*rayorig.DotProduct(raydir);
	float c = rayorig.DotProduct(rayorig)-radius*radius;

	//
	// Calc determinant
	float d = (b*b)-(4*a*c);
	if (d<0)
	{
		// No intersection
		return std::pair<bool, float>(false, 0);
	}
	else
	{
		// if d=0 there is one intersection
		// if d > 0 there are 2
		// But we only want the closest one, so that's ok, just use the 
		// '-' version of the solver
		float t = (-b - sqrt(d) )/(2*a);
		if (t<0)
		{
			t = (-b + sqrt(d))/(2*a);
		}
		return std::pair<bool,float>(true,t);
	}

}

std::pair<bool, float> glmath::Ray::Intersects( const BoundingBox& box ) const
{
	float lowt = 0.0f;
	float t;
	bool hit = false;
	Vector3 hitpoint;
	const Vector3& min = box.GetMin();
	const Vector3& max = box.GetMax();
	const Vector3& rayorig = this->GetOrigin();
	const Vector3& raydir = this->GetDirection();

	// Check if origin inside first
	if ( rayorig > min && rayorig < max )
	{
		return std::pair<bool, float>(true, 0);
	}

	// Check each face in turn, only check closest 3
	// Min x
	if (rayorig.x <= min.x && raydir.x > 0)
	{
		t = (min.x - rayorig.x) / raydir.x;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.y >= min.y && hitpoint.y <= max.y &&
				hitpoint.z >= min.z && hitpoint.z <= max.z &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}
	// Max x
	if (rayorig.x >= max.x && raydir.x < 0)
	{
		t = (max.x - rayorig.x) / raydir.x;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.y >= min.y && hitpoint.y <= max.y &&
				hitpoint.z >= min.z && hitpoint.z <= max.z &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}
	// Min y
	if (rayorig.y <= min.y && raydir.y > 0)
	{
		t = (min.y - rayorig.y) / raydir.y;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
				hitpoint.z >= min.z && hitpoint.z <= max.z &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}
	// Max y
	if (rayorig.y >= max.y && raydir.y < 0)
	{
		t = (max.y - rayorig.y) / raydir.y;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
				hitpoint.z >= min.z && hitpoint.z <= max.z &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}
	// Min z
	if (rayorig.z <= min.z && raydir.z > 0)
	{
		t = (min.z - rayorig.z) / raydir.z;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
				hitpoint.y >= min.y && hitpoint.y <= max.y &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}
	// Max z
	if (rayorig.z >= max.z && raydir.z < 0)
	{
		t = (max.z - rayorig.z) / raydir.z;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
				hitpoint.y >= min.y && hitpoint.y <= max.y &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}

	return std::pair<bool, float>(hit, lowt);
}

// std::pair<bool, float> glmath::Ray::Intersects( const BoundingBox& box ) const
// {
// 	//float lowt=0.0f;
// 	float t = 0;
// 	std::pair<bool,float> result;
// 	bool hit=false;
// 	//const Vector3& min = box;
// 	const Vector3& rayorig = this->GetOrigin() - box.GetCenter();	//bring the ray to the box's local axis system
// 	const Vector3& raydir = this->GetDirection() - box.GetCenter();
// 
// 	// Check if origin inside first
// 	if (box.IsPointinside(this->GetOrigin()))
// 	{
// 		return std::pair<bool, float>(true, 0);
// 	}
// 
// 	// check against each side, keep the closest one
// 	Plane p(Vector3::UNIT_X, box.GetXDim());
// 	result = this->Intersects(p);
// 	if( result.first==true )
// 	{
// 		t = result.second;
// 		hit=true;
// 	}
// 
// 	p.SetNormal(Vector3::NEGATIVE_UNIT_X);
// 	p.SetIntercept(-box.GetXDim());
// 	result = this->Intersects(p);
// 	if ( result.first )
// 	{
// 		hit=true;
// 		if (result.second<t || t==0)
// 		{
// 			t = result.second;
// 		}
// 	}
// 
// 	p.SetNormal(Vector3::UNIT_Y);
// 	p.SetIntercept(box.GetYDim());
// 	result = this->Intersects(p);
// 	if ( result.first )
// 	{
// 		hit=true;
// 		if (result.second<t || t==0)
// 		{
// 			t = result.second;
// 		}
// 	}
// 
// 	p.SetNormal(Vector3::UNIT_Y);
// 	p.SetIntercept(box.GetYDim());
// 	result = this->Intersects(p);
// 	if ( result.first )
// 	{
// 		hit=true;
// 		if (result.second<t || t==0)
// 		{
// 			t = result.second;
// 		}
// 	}
// 
// 	p.SetNormal(Vector3::NEGATIVE_UNIT_Y);
// 	p.SetIntercept(-box.GetYDim());
// 	result = this->Intersects(p);
// 	if ( result.first )
// 	{
// 		hit=true;
// 		if (result.second<t || t==0)
// 		{
// 			t = result.second;
// 		}
// 	}
// 
// 	p.SetNormal(Vector3::UNIT_Z);
// 	p.SetIntercept(box.GetZDim());
// 	result = this->Intersects(p);
// 	if ( result.first )
// 	{
// 		hit=true;
// 		if (result.second<t || t==0)
// 		{
// 			t = result.second;
// 		}
// 	}
// 
// 	return std::pair<bool,float>((t>0), t);
// }	

bool glmath::Vector3::operator<( const Vector3& v ) const
{
	if( this->x<v.x && this->y<v.y && this->z<v.z )
		return true;
	return false;
}

bool glmath::Vector3::operator>( const Vector3& v ) const
{
	if( this->x>v.x && this->y>v.y && this->z>v.z )
		return true;
	return false;
}